#ifndef MT6701_SENSOR_H
#define MT6701_SENSOR_H

#include "spi.h"
#include "gpio.h"
#include <stdint.h>
#include <bits/range_access.h>
#include <../motor/Motor.h>
#define _PI       3.14159265359f
#define _2PI      6.28318530718f
#define _3PI_2    4.71238898038f

struct MT6701SensorData {
    //角度
    float angle;
    //速度
    float velocity;
    //步骤
    uint32_t step;
};

class MT6701Sensor {
public:
    // 获取单例实例的静态方法
    static MT6701Sensor* getInstance();

    MT6701Sensor(SPI_HandleTypeDef *spi, GPIO_TypeDef *csPort, uint16_t csPin, uint8_t polePairs);

    /**
     * 原始数据
     * @return 
     */
    uint16_t getRawData();

    /**
     * 
     * @return 
     */
    float getRawAngle(); // 无圈数
    float getFullAngle(); // 有圈数
    float getVelocity(); // 转速
    float getElectricalAngle(); // 电角度
    void alignSensor(); // 零位校准

    inline float _normalizeAngle(float angle) {
        while (angle < 0) angle += _2PI;
        while (angle >= _2PI) angle -= _2PI;
        return angle;
    }

    uint8_t getCommutationStep();

    MT6701SensorData& getData() {
        return sensorData;
    }

    /**
     * 滤波后的步
     * @return
     */
    uint8_t getFilteredStep() ;



private:
    SPI_HandleTypeDef *_spi;
    GPIO_TypeDef *_csPort;
    uint16_t _csPin;
    uint8_t _polePairs;

    float _zeroElectricalOffset = 0.0f;
    int _rotationCount = 0;
    int _rotationCountLast = 0;
    float _angleLast = 0.0f;
    float _fullAngleLast = 0.0f;
    float alignment_offset;

    void csEnable();

    void csDisable();

    MT6701SensorData sensorData;

    // 私有静态实例
    static MT6701Sensor* instance;

    CommutationStep currentStep ;
};

#endif // MT6701_SENSOR_H
